// --------------------------------------------------------------------------------------------------------------------
// <copyright file="CoordinateTransform2D.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Numerics
{
    using System;
    using System.Diagnostics;
    using System.Runtime.CompilerServices;
    using System.Runtime.Serialization;

    /// <summary>
    /// Struct used to cache the values needed for an affine transform applied to an image or image-like structure.
    /// </summary>
    /// <remarks>
    /// This is a struct because the typical usage is to instantiate it right outside a for loop and use it within the for loop, 
    /// so having all members on the stack to avoid the pointer indirection makes sense.
    /// </remarks>
    [DataContract]
    public struct CoordinateTransform2D
    {
        /// <summary>
        /// The components of the affine transform (missing third row is simply assumed to be 0, 0, 1)
        /// </summary>
        [DataMember]
        private double a11, a12, a13, a21, a22, a23;

        /// <summary>
        /// Initializes a new instance of the CoordinateTransform2D struct.
        /// The resulting instance can be used to transform points from one coordinate frame to another (rigid transformation).
        /// </summary>
        /// <param name="poseOfOldOriginInNewSystem">The coordinate frame to transform to (rotate + translate), specified as position + orientation of the old origin in the new system.</param>
        public CoordinateTransform2D(Pose2D poseOfOldOriginInNewSystem)
            : this(poseOfOldOriginInNewSystem, 1)
        {
        }

        /// <summary>
        /// Initializes a new instance of the CoordinateTransform2D struct.
        /// The resulting instance can be used to transform points from one coordinate frame to another (rigid transformation).
        /// </summary>
        /// <param name="poseOfOldOriginInNewSystem">The coordinate frame to transform to (rotate + translate + scale), specified as position + orientation of the old origin in the new system.</param>
        /// <param name="scale">A scale factor to apply after the transformation</param>
        public CoordinateTransform2D(Pose2D poseOfOldOriginInNewSystem, double scale)
        {
            double sin = Math.Sin(poseOfOldOriginInNewSystem.Heading);
            double cos = Math.Cos(poseOfOldOriginInNewSystem.Heading);
            this.a11 = cos * scale;
            this.a12 = -sin * scale;
            this.a13 = poseOfOldOriginInNewSystem.X;
            this.a21 = sin * scale;
            this.a22 = cos * scale;
            this.a23 = poseOfOldOriginInNewSystem.Y;
        }

        /// <summary>
        /// Initializes a new instance of the CoordinateTransform2D struct given a transformation matrix
        /// </summary>
        /// <param name="transformationMatrix">The transformation matrix (third row is ignored)</param>
        public CoordinateTransform2D(Matrix3 transformationMatrix)
        {
            this.a11 = transformationMatrix[0, 0];
            this.a12 = transformationMatrix[0, 1];
            this.a13 = transformationMatrix[0, 2];
            this.a21 = transformationMatrix[1, 0];
            this.a22 = transformationMatrix[1, 1];
            this.a23 = transformationMatrix[1, 2];
        }

        /// <summary>
        /// The equals operator
        /// </summary>
        /// <param name="leftSide">Transform on the left side</param>
        /// <param name="rightSide">Transform on the right side</param>
        /// <returns>Are the Transform values equal</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator ==(CoordinateTransform2D leftSide, CoordinateTransform2D rightSide)
        {
            // Return true if all the fields match:
            return 
                leftSide.a11 == rightSide.a11 && leftSide.a12 == rightSide.a12 && leftSide.a13 == rightSide.a13 &&
                leftSide.a21 == rightSide.a21 && leftSide.a22 == rightSide.a22 && leftSide.a23 == rightSide.a23;
        }

        /// <summary>
        /// The not equals operator
        /// </summary>
        /// <param name="leftSide">Transform on the left side</param>
        /// <param name="rightSide">Transform on the right side</param>
        /// <returns>The not equal result</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator !=(CoordinateTransform2D leftSide, CoordinateTransform2D rightSide)
        {
            // Return true if any fields don't match
            return leftSide.a11 != rightSide.a11 || leftSide.a12 != rightSide.a12 || leftSide.a13 != rightSide.a13 ||
                leftSide.a21 != rightSide.a21 || leftSide.a22 != rightSide.a22 || leftSide.a23 != rightSide.a23;
        }

        /// <summary>
        /// Creates a reverse transform
        /// </summary>
        /// <returns>A transform that can be used to transform points in the opposite direction</returns>
        public CoordinateTransform2D Reverse()
        {
            Matrix3 m = this.GetTransformationMatrix().Inverse();
            return new CoordinateTransform2D(m);
        }

        /// <summary>
        /// Combines two transforms, such that the current transform is applied first and the passed in one is applied second
        /// </summary>
        /// <param name="second">The transform to apply after the current one</param>
        /// <returns>A combined transform</returns>
        public CoordinateTransform2D Append(CoordinateTransform2D second)
        {
            Matrix3 m1 = this.GetTransformationMatrix();
            Matrix3 m2 = second.GetTransformationMatrix();
            Matrix3 p = m2 * m1;
            CoordinateTransform2D combined = new CoordinateTransform2D(p);
            return combined;
        }

        /// <summary>
        /// Applies an affine transform to the specified point.
        /// </summary>
        /// <param name="point">The point to transform.</param>
        /// <returns>The result of the affine transformation.</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Point2D Transform(Point2D point)
        {
            return this.Transform(point.X, point.Y);
        }

        /// <summary>
        /// Applies an affine transform to the specified pose.        
        /// </summary>
        /// <param name="pose">The pose to transform.</param>
        /// <returns>The result of the affine transformation.</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Pose2D Transform(Pose2D pose)
        {
            double xt = pose.X * this.a11 + pose.Y * this.a12 + this.a13;
            double yt = pose.X * this.a21 + pose.Y * this.a22 + this.a23;

            double cos = Math.Cos(pose.Heading);
            double sin = Math.Sin(pose.Heading);

            return new Pose2D(xt, yt, Math.Atan2(this.a21 * cos + this.a22 * sin, this.a11 * cos + this.a12 * sin));
        }

        /// <summary>
        /// Applies an affine transform to the specified point.
        /// </summary>
        /// <param name="x">The x coordinate of the point to transform</param>
        /// <param name="y">The y coordinate of the point to transform</param>
        /// <returns>The result of the affine transformation.</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Point2D Transform(double x, double y)
        {
            double xt = x * this.a11 + y * this.a12 + this.a13;
            double yt = x * this.a21 + y * this.a22 + this.a23;
            return new Point2D(xt, yt);
        }

        /// <summary>
        /// Get hash code value.
        /// </summary>
        /// <returns>Hash code.</returns>
        public override int GetHashCode()
        {
            return 
                ((this.a11.GetHashCode() ^ this.a12.GetHashCode() ^ this.a13.GetHashCode()) << 16) ^ 
                this.a21.GetHashCode() ^ this.a22.GetHashCode() ^ this.a23.GetHashCode();
        }

        /// <summary>
        /// Determine equality with another object.
        /// </summary>
        /// <param name="obj">Another Transform object to which to compare.</param>
        /// <returns>Indication of equality.</returns>
        public override bool Equals(object obj)
        {
            CoordinateTransform2D other = (CoordinateTransform2D)obj;
            return this == other;
        }

        /// <summary>
        /// Returns the internal transformation matrix
        /// </summary>
        /// <returns>The matrix representing the transformation</returns>
        public Matrix3 GetTransformationMatrix()
        {
            return new Matrix3(new[,] { { this.a11, this.a12, this.a13 }, { this.a21, this.a22, this.a23 }, { 0, 0, 1 } });
        }
    }
}
